#ifndef OBB_H
#define OBB_H
#include <cmath>
#include <iostream>
#include <vector>
namespace perception
{

    namespace algorithm
    {
        class OBB
        {
        public:
            struct Box2d
            {
                double x, y;
                double heading;
                double length, width;
            };

        public:
            static bool HasOverlap(const Box2d &box1, const Box2d &box2)
            {

                const double shift_x = box1.x - box2.x;
                const double shift_y = box1.y - box2.y;
                double sin_heading_ = std::sin(box1.heading);
                double cos_heading_ = std::cos(box1.heading);
                const double dx1 = std::cos(box1.heading) * box1.length / 2;
                const double dy1 = sin_heading_ * box1.length / 2;
                const double dx2 = sin_heading_ * box1.width / 2;
                const double dy2 = -cos_heading_ * box1.width / 2;
                const double dx3 = std::cos(box2.heading) * box2.length / 2;
                const double dy3 = std::sin(box2.heading) * box2.length / 2;
                const double dx4 = std::sin(box2.heading) * box2.width / 2;
                const double dy4 = -std::cos(box2.heading) * box2.width / 2;

                // 在分别往 ego和obstacle 的横纵轴上进行投影
                return std::abs(shift_x * cos_heading_ + shift_y * sin_heading_) <=
                           std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) +
                               std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) +
                               box1.length / 2 &&
                       std::abs(shift_x * sin_heading_ - shift_y * cos_heading_) <=
                           std::abs(dx3 * sin_heading_ - dy3 * cos_heading_) +
                               std::abs(dx4 * sin_heading_ - dy4 * cos_heading_) +
                               box1.width / 2 &&
                       std::abs(shift_x * std::cos(box2.heading) + shift_y * std::sin(box2.heading)) <=
                           std::abs(dx1 * std::cos(box2.heading) + dy1 * std::sin(box2.heading)) +
                               std::abs(dx2 * std::cos(box2.heading) + dy2 * std::sin(box2.heading)) +
                               box2.length / 2 &&
                       std::abs(shift_x * std::sin(box2.heading) - shift_y * std::cos(box2.heading)) <=
                           std::abs(dx1 * std::sin(box2.heading) - dy1 * std::cos(box2.heading)) +
                               std::abs(dx2 * std::sin(box2.heading) - dy2 * std::cos(box2.heading)) +
                               box2.width / 2;
            }
        };
    }
}
#endif